from flask import Flask, jsonify, request
from flask_cors import CORS
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from std_msgs.msg import Bool
import threading
import time
from enum import Enum

class CarState(Enum):
    STOP = "stop"
    FORWARD = "forward"
    BACKWARD = "backward"
    LEFT = "left"
    RIGHT = "right"
    TURN_LEFT = "turn_left"
    TURN_RIGHT = "turn_right"

class CarController(Node):
    def __init__(self):
        super().__init__('flask_car_controller')
        
        # 创建发布者
        self.pub_cmd_vel = self.create_publisher(Twist, '/cmd_vel', 1)
        self.pub_buzzer = self.create_publisher(Bool, '/Buzzer', 1)
        
        # 运动参数
        self.linear_speed = 0.3   # 线速度 (m/s)
        self.angular_speed = 0.8  # 角速度 (rad/s)
        
        # 当前状态
        self.current_state = CarState.STOP
        self.buzzer_state = False
        
        print("✅ 小车控制节点初始化完成")
    
    def _publish_twist(self, linear_x=0.0, linear_y=0.0, angular_z=0.0):
        """发布运动指令"""
        twist = Twist()
        twist.linear.x = linear_x
        twist.linear.y = linear_y
        twist.angular.z = angular_z
        self.pub_cmd_vel.publish(twist)
    
    def _execute_movement(self, new_state, linear_x=0.0, linear_y=0.0, angular_z=0.0):
        """执行运动指令（如果当前不是停止状态，先停止再执行）"""
        if self.current_state != CarState.STOP:
            # 先停止
            self._publish_twist(0.0, 0.0, 0.0)
            time.sleep(0.1)  # 短暂停顿确保停止
            
        # 执行新的运动指令
        self._publish_twist(linear_x, linear_y, angular_z)
        self.current_state = new_state
    
    def move_forward(self):
        """前进"""
        self._execute_movement(CarState.FORWARD, self.linear_speed, 0.0, 0.0)
        print("🔼 前进")
    
    def move_backward(self):
        """后退"""
        self._execute_movement(CarState.BACKWARD, -self.linear_speed, 0.0, 0.0)
        print("🔽 后退")
    
    def move_left(self):
        """左平移"""
        self._execute_movement(CarState.LEFT, 0.0, self.linear_speed, 0.0)
        print("◀️ 左平移")
    
    def move_right(self):
        """右平移"""
        self._execute_movement(CarState.RIGHT, 0.0, -self.linear_speed, 0.0)
        print("▶️ 右平移")
    
    def turn_left(self):
        """左转"""
        self._execute_movement(CarState.TURN_LEFT, 0.0, 0.0, self.angular_speed)
        print("↺ 左转")
    
    def turn_right(self):
        """右转"""
        self._execute_movement(CarState.TURN_RIGHT, 0.0, 0.0, -self.angular_speed)
        print("↻ 右转")
    
    def stop(self):
        """停止"""
        self._publish_twist(0.0, 0.0, 0.0)
        self.current_state = CarState.STOP
        print("⏹️ 停止")
    
    def buzzer_on(self):
        """开启蜂鸣器"""
        buzzer_msg = Bool()
        buzzer_msg.data = True
        self.pub_buzzer.publish(buzzer_msg)
        self.buzzer_state = True
        print("🔊 蜂鸣器开启")
    
    def buzzer_off(self):
        """关闭蜂鸣器"""
        buzzer_msg = Bool()
        buzzer_msg.data = False
        self.pub_buzzer.publish(buzzer_msg)
        self.buzzer_state = False
        print("🔇 蜂鸣器关闭")
    
    def get_status(self):
        """获取当前状态"""
        return {
            'state': self.current_state.value,
            'buzzer': self.buzzer_state,
            'linear_speed': self.linear_speed,
            'angular_speed': self.angular_speed
        }

# 全局变量
car_controller = None
ros_thread = None

def ros_spin_thread():
    """ROS2循环线程"""
    while rclpy.ok():
        rclpy.spin_once(car_controller, timeout_sec=0.1)

# 创建Flask应用
app = Flask(__name__)
CORS(app)  # 启用跨域支持

@app.route('/api/control', methods=['POST'])
def control():
    """控制命令接口"""
    try:
        data = request.json
        command = data.get('command')
        
        if command == 'forward':
            car_controller.move_forward()
            return jsonify({'status': 'success', 'message': '前进'})
        elif command == 'backward':
            car_controller.move_backward()
            return jsonify({'status': 'success', 'message': '后退'})
        elif command == 'left':
            car_controller.move_left()
            return jsonify({'status': 'success', 'message': '左平移'})
        elif command == 'right':
            car_controller.move_right()
            return jsonify({'status': 'success', 'message': '右平移'})
        elif command == 'turn_left':
            car_controller.turn_left()
            return jsonify({'status': 'success', 'message': '左转'})
        elif command == 'turn_right':
            car_controller.turn_right()
            return jsonify({'status': 'success', 'message': '右转'})
        elif command == 'stop':
            car_controller.stop()
            return jsonify({'status': 'success', 'message': '停止'})
        elif command == 'buzzer_on':
            car_controller.buzzer_on()
            return jsonify({'status': 'success', 'message': '蜂鸣器开启'})
        elif command == 'buzzer_off':
            car_controller.buzzer_off()
            return jsonify({'status': 'success', 'message': '蜂鸣器关闭'})
        else:
            return jsonify({'status': 'error', 'message': '未知命令'})
            
    except Exception as e:
        return jsonify({'status': 'error', 'message': str(e)})

@app.route('/api/set_speed', methods=['POST'])
def set_speed():
    """设置线速度和角速度"""
    try:
        data = request.json
        linear_speed = data.get('linear_speed')
        angular_speed = data.get('angular_speed')
        
        # 验证参数
        if linear_speed is not None:
            if not isinstance(linear_speed, (int, float)) or linear_speed < 0 or linear_speed > 2.0:
                return jsonify({'status': 'error', 'message': '线速度必须在0-2.0之间'})
            car_controller.linear_speed = float(linear_speed)
        
        if angular_speed is not None:
            if not isinstance(angular_speed, (int, float)) or angular_speed < 0 or angular_speed > 3.0:
                return jsonify({'status': 'error', 'message': '角速度必须在0-3.0之间'})
            car_controller.angular_speed = float(angular_speed)
        
        # 如果没有提供任何参数
        if linear_speed is None and angular_speed is None:
            return jsonify({'status': 'error', 'message': '请提供linear_speed或angular_speed参数'})
        
        message = f"速度已更新 - 线速度: {car_controller.linear_speed} m/s, 角速度: {car_controller.angular_speed} rad/s"
        print(f"⚡ {message}")
        
        return jsonify({
            'status': 'success', 
            'message': message,
            'data': {
                'linear_speed': car_controller.linear_speed,
                'angular_speed': car_controller.angular_speed
            }
        })
    except Exception as e:
        return jsonify({'status': 'error', 'message': str(e)})

@app.route('/api/status', methods=['GET'])
def get_status():
    """获取当前状态"""
    try:
        status = car_controller.get_status()
        return jsonify({'status': 'success', 'data': status})
    except Exception as e:
        return jsonify({'status': 'error', 'message': str(e)})

@app.route('/api/health', methods=['GET'])
def health_check():
    """健康检查"""
    return jsonify({'status': 'success', 'message': 'API服务正常运行'})

def main():
    global car_controller, ros_thread
    
    print("🚀 启动Flask小车控制API服务器")
    
    # 初始化ROS2
    rclpy.init()
    car_controller = CarController()
    
    # 启动ROS2循环线程
    ros_thread = threading.Thread(target=ros_spin_thread, daemon=True)
    ros_thread.start()
    
    print("🌐 Flask API服务器启动中...")
    print("📡 API端点:")
    print("   - POST /api/control - 控制命令")
    print("   - POST /api/set_speed - 设置速度")
    print("   - GET /api/status - 获取状态")
    print("   - GET /api/health - 健康检查")
    print("🔗 服务器地址: http://localhost:5000")
    
    try:
        # 启动Flask应用
        app.run(host='0.0.0.0', port=5001, debug=False, threaded=True)
    except KeyboardInterrupt:
        print("\n🛑 程序被用户中断")
    finally:
        # 清理
        if car_controller:
            car_controller.stop()
        if rclpy.ok():
            rclpy.shutdown()
        print("✅ 程序已退出")

if __name__ == '__main__':
    main()